#include <stdio.h>
#include <unistd.h>

#include "main.h"
#include "Qnx.h"
#include "userInterface.h"

#define MOTOR_ON	1
#define MOTOR_OFF	0

/********************************************************************
* This file initializes all of the hardware, I/O features,    		*
* and the user interface. For details on the actual functionality	*
* see the individual files. The overall structure of this project	*
* is to have a control loop running which drives the motor and		*
* communicates with the high-speed vision system for object			*
* tracking. 														*
********************************************************************/

// create global pointers so they can be referenced by other functions
IoHardware			*HW;	// has all the hardware IO capabilities
SampleLoopTask		*SampleLoop;	// periodic control loop
MatlabNet			*MNET;	// network communication with MATLAB
ExternalInterrupt 	*ExtInt;	// external interrupt

double ActualSampleRate;

int main(){
	int i;
	sem_t block;

	// Enable Hardware Access
	ThreadCtl(_NTO_TCTL_IO, NULL);
	
	// Instantiate and Configure Global Modules
	
	// Signal registration is done in individual module's Init()'s
	MNET = new MatlabNet();
	
	// Hardware management module
	HW = new IoHardware();
	HW->Init();
	
	// Make sure all digital io processing settles down
	for(int i=0; i<20; i++){
		HW->ProcessInput();
	}
	
	// Initialize external interrupt command
	ExtInt = new ExternalInterrupt();
	ExtInt->Init("External Interrupt", 65, 10);
	
	//Enable external interrupt on port C, bit 3 of group 1
	HW->EnableExternalInterrupt();
	HW->ClearExternalInterrupt();
	
	// Instantiate Tasks
	SampleLoop = new SampleLoopTask();
	SampleLoop->Init("SampleLoop", SAMPLE_RATE, &ActualSampleRate, 60);
	
	// Start up MatlabNet after all the signals have been added
	MNET->Init(ActualSampleRate, 14);
	
	// Set up digital output to enable motor
	HW->WriteDigitalBit(IoHardware::MOTOR_ENABLE, MOTOR_ON);
	
	// Start user interface loop
	// exits when user hits 'q'
	ui();
	
	// Set motor output to zero and disable the amplifier
	SampleLoop->done = 1;
	HW->WriteDigitalBit(IoHardware::MOTOR_ENABLE, MOTOR_OFF);
	HW->SetAnalogOut(IoHardware::AMP_SIGNAL, 0.0);
	
	delay(10);
}
